# -*- coding:utf-8 -*-
from ctypes import *
import time
import numpy as np
import mujoco

# 导入自定义函数和CANFD SDK动态链接库>>>
from os.path import dirname, join
import sys
import platform
arch = platform.machine()
sys.path.append(join(dirname(__file__), 'src/lysdemo_'+arch))
sys.path.append(join(dirname(__file__), "src"))
sys.path.append(dirname(__file__))
from src import *

# ###########################################################################


if __name__ == "__main__":
    
    # >>>>>初始化CAN卡部分>>>
    # 设置can or canfd接口
    lcanlib = LCAN()
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)

    # open device
    ret = lcanlib.LCAN_OpenDevice(LCAN_USBCANFD2, devIndex)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANFD2 Open Device failed!")
        exit(0)
    # open success
    print("LYS USBCANFD2 Open Device Success!")


    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)

    # 初始化canfd
    ret = lcanlib.LCAN_InitCANFD(LCAN_USBCANFD2, devIndex, canIndex, canfd_init_cfg)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANFD2 Init Failed!")
    else:
        print("LYS USBCANFD2 Init Success!")
    

    # 清除can通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)
    print('sleeping')
    time.sleep(0.1)
    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    motor_controllers = MotorController(motor_num)

    for ID in range(motor_num):
        

        # 在线更新标志位
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.can_obj_init[ID], 1)
        print('sleeping')
        time.sleep(0.05)
        if ret != LCAN_STATUS_OK:
            print(f"motor {ID} init Failed!")
        else:
            print(f"motor {ID} init Success!")



        # 设置p模式
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.can_p_mode[ID], 1)
        print('sleeping')
        time.sleep(0.05)
        if ret != LCAN_STATUS_OK:
            print(f"Set {ID} p mode failed!")
        else:
            print(f"Set {ID} p mode success!")


    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # 控制
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)

    # 发送与读取>>>
    # P Mode

    pvc = np.zeros((motor_num,3))
    # 每个周期，发送的目标位置和当前角度误差，最大不超过max_delta_intq

    max_delta_intq_eachloop = min(int(rad_per_sec*control_period),max_dq)

    time.sleep(3)

    
    start = time.time()

    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.can_obj_get, motor_num)
    time.sleep(0.5)
    q_real = receive_p_state(lcanlib, devIndex, canIndex)

    # 读取初始点位
    model = mujoco.MjModel.from_xml_path(join(dirname(__file__),'scene_7DOF.xml')) 
    data = mujoco.MjData(model)
    mocap_id = model.body("target").mocapid[0]
    mocap_pos = data.mocap_pos[mocap_id]
    mocap_quat = data.mocap_quat[mocap_id]
    # import math
    while time.time() - start < control_time:
        loop_start = time.time()

        q_real_float = trans_intq2q(q_real)
        # force = np.zeros(6)+math.sin(2*(time.time() - start))
        # print(f"force is {force}")
        ikq_star = ik7(q_real_float, mocap_pos, mocap_quat)
        intikq = trans_q2intq(ikq_star) # 最终目标点（也可能是IK尚未收敛的中间变量）

        # 归零
        # intikq = np.array([0,0,0,0,0,0,0])

        # print(f"q_real = {q_real}")
        # print(f"intikq_clip = {intikq_clip}")
        print(f"intikq = {intikq}")
        print(f"q_real = {q_real}")

        ctrl_intq = get_interpoint(q_real, intikq, max_dq)

        print(f"ctrl_intq = {ctrl_intq}")

        motor_controllers.set_p(ctrl_intq)
        # 发送指令
        # transfer_time1 = time.time()
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.can_set_p, motor_num)
        # print(f"transfer time cost: {time.time() - transfer_time1}")

        # 读取状态
        new_pvc = receive_pvc(lcanlib, devIndex, canIndex)

        if new_pvc is not None:
            pvc = new_pvc  # 仅当有有效数据时更新
            print(f"最新状态:\n {pvc}")
        else:
            print(f"get no pvc data\n")

        q_real = pvc.T[0]

        elapsed_time = time.time() - loop_start
        sleep_time = control_period - elapsed_time
        if sleep_time > 0:
            time.sleep(sleep_time)
            print(f"loop time cost: {elapsed_time}")
        else:
            print(f"loop overtime: {-sleep_time}")


    # 发送与读取<<<

    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    #Close Device
    lcanlib.LCAN_CloseDevice(LCAN_USBCANFD2, devIndex)
    print("LYS USBCANFD2 Close!")